#include <caros/ur_ai_proxy.h>

#include <caros/ur_action_interface.h>
#include <caros/common.h>
#include <caros/common_robwork.h>
#include <string>
#include <vector>

#define SPEED_MIN 0.0f
#define SPEED_MAX 1.0f
#define ACCEL_MIN 0.0f
#define ACCEL_MAX 2.0f
#define BLEND_MIN 0.0f
#define BLEND_MAX 2.0f
#define SERVO_LOOKAHEAD_TIME_MIN 0.03f
#define SERVO_LOOKAHEAD_TIME_MAX 0.2f
#define SERVO_GAIN_MIN 100
#define SERVO_GAIN_MAX 2000

using caros::URAIProxy;

URAIProxy::URAIProxy(ros::NodeHandle nodehandle, const std::string& devname, const bool blocking)
    : nodehandle_(nodehandle),
      blocking_(blocking),

      ros_namespace_("/" + devname + "/" + UR_ACTION_INTERFACE_SUB_NAMESPACE + "/"),
      ac_move_force_mode_start_(ros_namespace_ + "force_mode_start", USE_THREAD),
      ac_move_force_mode_update_(ros_namespace_ + "force_mode_update", USE_THREAD),
      ac_move_force_mode_stop_(ros_namespace_ + "force_mode_stop", USE_THREAD),
      ac_move_lin_(ros_namespace_ + "move_lin", USE_THREAD),
      ac_move_ptp_(ros_namespace_ + "move_ptp", USE_THREAD),
      ac_move_ptp_path_(ros_namespace_ + "move_ptp_path", USE_THREAD),
      ac_move_servo_q_(ros_namespace_ + "move_servo_q", USE_THREAD),
      ac_move_servo_stop_(ros_namespace_ + "move_servo_stop", USE_THREAD),
      ac_move_vel_q_(ros_namespace_ + "move_vel_q", USE_THREAD),
      ac_move_vel_t_(ros_namespace_ + "move_vel_t", USE_THREAD),
      ac_set_io_(ros_namespace_ + "set_io", USE_THREAD),
      ac_set_payload_(ros_namespace_ + "set_payload", USE_THREAD),
      ac_zero_ft_sensor_(ros_namespace_ + "zero_ftsensor", USE_THREAD),
      ac_send_custom_script_function_(ros_namespace_ + "send_custom_script_function", USE_THREAD),
      ac_send_custom_script_file_(ros_namespace_ + "send_custom_script_file", USE_THREAD)
{
  // states
  sub_robot_state_ = nodehandle_.subscribe(ros_namespace_ + "/robot_state", 1, &URAIProxy::handleRobotState, this);

  ROS_INFO("Waiting for UR action servers to start.");
  ac_move_force_mode_start_.waitForServer();
  ac_move_force_mode_update_.waitForServer();
  ac_move_force_mode_stop_.waitForServer();
  ac_move_lin_.waitForServer();
  ac_move_ptp_.waitForServer();
  ac_move_ptp_path_.waitForServer();
  ac_move_servo_q_.waitForServer();
  ac_move_servo_stop_.waitForServer();
  ac_move_vel_q_.waitForServer();
  ac_move_vel_t_.waitForServer();
  ac_set_io_.waitForServer();
  ac_set_payload_.waitForServer();
  ac_zero_ft_sensor_.waitForServer();
  ac_send_custom_script_function_.waitForServer();
  ac_send_custom_script_file_.waitForServer();
  ROS_INFO("UR action servers started");
}

URAIProxy::~URAIProxy()
{
}

bool URAIProxy::moveForceModeStart(const rw::math::Transform3D<>& ref_t_offset, const rw::math::Q& selection,
                                   const rw::math::Wrench6D<>& wrench_target, int type, const rw::math::Q& limits)
{
  bool finished_before_timeout = true;

  caros_universalrobot::ForceModeStartGoal force_mode_start_goal;
  force_mode_start_goal.base2forceFrame = caros::toRos(ref_t_offset);
  std::vector<uint32_t> selection_vec;
  for (size_t i = 0; i < selection.size(); i++)
  {
    selection_vec.push_back(static_cast<uint32_t>(selection[i]));
  }
  force_mode_start_goal.selection = selection_vec;
  force_mode_start_goal.wrench = caros::toRos(wrench_target);
  force_mode_start_goal.type = type;
  std::vector<float> limits_vec;
  for (size_t i = 0; i < limits.size(); i++)
  {
    limits_vec.push_back(static_cast<float>(limits[i]));
  }
  force_mode_start_goal.limits = limits_vec;

  ac_move_force_mode_start_.sendGoal(force_mode_start_goal);

  if (blocking_)
  {
    finished_before_timeout = ac_move_force_mode_start_.waitForResult(ros::Duration(UR_EXECUTION_TIMEOUT));
    if (finished_before_timeout)
    {
      actionlib::SimpleClientGoalState state = ac_move_force_mode_start_.getState();
      ROS_INFO("Action finished: %s", state.toString().c_str());
    }
    else
      ROS_INFO("Action did not finish before the time out.");
  }

  return finished_before_timeout;
}

bool URAIProxy::moveForceModeUpdate(const rw::math::Wrench6D<>& wrench_target)
{
  bool finished_before_timeout = true;

  caros_universalrobot::ForceModeUpdateGoal force_mode_update_goal;
  force_mode_update_goal.wrench = caros::toRos(wrench_target);

  ac_move_force_mode_update_.sendGoal(force_mode_update_goal);

  if (blocking_)
  {
    finished_before_timeout = ac_move_force_mode_update_.waitForResult(ros::Duration(UR_EXECUTION_TIMEOUT));
    if (finished_before_timeout)
    {
      actionlib::SimpleClientGoalState state = ac_move_force_mode_update_.getState();
      ROS_INFO("Action finished: %s", state.toString().c_str());
    }
    else
      ROS_INFO("Action did not finish before the time out.");
  }

  return finished_before_timeout;
}

bool URAIProxy::moveForceModeStop()
{
  bool finished_before_timeout = true;

  caros_universalrobot::ForceModeStopGoal force_mode_stop_goal;

  ac_move_force_mode_stop_.sendGoal(force_mode_stop_goal);

  if (blocking_)
  {
    finished_before_timeout = ac_move_force_mode_stop_.waitForResult(ros::Duration(UR_EXECUTION_TIMEOUT));
    if (finished_before_timeout)
    {
      actionlib::SimpleClientGoalState state = ac_move_force_mode_stop_.getState();
      ROS_INFO("Action finished: %s", state.toString().c_str());
    }
    else
      ROS_INFO("Action did not finish before the time out.");
  }

  return finished_before_timeout;
}

bool URAIProxy::moveLin(const rw::math::Transform3D<>& target, const float speed, const float acceleration)
{
  bool finished_before_timeout = true;

  verifyValueIsWithin(speed, SPEED_MIN, SPEED_MAX);
  verifyValueIsWithin(acceleration, ACCEL_MIN, ACCEL_MAX);
  caros_universalrobot::MoveLinGoal move_lin_goal;
  move_lin_goal.tcp_pose = caros::toRos(target);
  move_lin_goal.speed = caros::toRos(speed);
  move_lin_goal.tool_acceleration = caros::toRos(acceleration);

  ac_move_lin_.sendGoal(move_lin_goal);

  if (blocking_)
  {
    finished_before_timeout = ac_move_lin_.waitForResult(ros::Duration(UR_EXECUTION_TIMEOUT));
    if (finished_before_timeout)
    {
      actionlib::SimpleClientGoalState state = ac_move_lin_.getState();
      ROS_INFO("Action finished: %s", state.toString().c_str());
    }
    else
      ROS_INFO("Action did not finish before the time out.");
  }

  return finished_before_timeout;
}

bool URAIProxy::movePtp(const rw::math::Q& target, const float speed, const float acceleration)
{
  bool finished_before_timeout = true;

  verifyValueIsWithin(speed, SPEED_MIN, SPEED_MAX);
  verifyValueIsWithin(acceleration, ACCEL_MIN, ACCEL_MAX);
  caros_universalrobot::MovePtpGoal move_ptp_goal;
  move_ptp_goal.joint_pose = caros::toRos(target);
  move_ptp_goal.speed = caros::toRos(speed);
  move_ptp_goal.acceleration = caros::toRos(acceleration);

  ac_move_ptp_.sendGoal(move_ptp_goal);

  if (blocking_)
  {
    finished_before_timeout = ac_move_ptp_.waitForResult(ros::Duration(UR_EXECUTION_TIMEOUT));
    if (finished_before_timeout)
    {
      actionlib::SimpleClientGoalState state = ac_move_ptp_.getState();
      ROS_INFO("Action finished: %s", state.toString().c_str());
    }
    else
      ROS_INFO("Action did not finish before the time out.");
  }

  return finished_before_timeout;
}

bool URAIProxy::movePtpPath(const rw::trajectory::QPath& q_path)
{
  bool finished_before_timeout = true;

  caros_universalrobot::MovePtpPathGoal move_ptp_path_goal;
  for (const auto& q : q_path)
  {
    rw::math::Q joint_q(6, q[0], q[1], q[2], q[3], q[4], q[5]);
    double speed = q[6];
    double acceleration = q[7];
    double blend = q[8];
    verifyValueIsWithin(speed, SPEED_MIN, SPEED_MAX);
    verifyValueIsWithin(acceleration, ACCEL_MIN, ACCEL_MAX);
    verifyValueIsWithin(blend, BLEND_MIN, BLEND_MAX);
    move_ptp_path_goal.joint_poses.push_back(caros::toRos(joint_q));
    move_ptp_path_goal.speeds.push_back(caros::toRos(speed));
    move_ptp_path_goal.accelerations.push_back(caros::toRos(acceleration));
    move_ptp_path_goal.blends.push_back(caros::toRos(blend));
  }

  ac_move_ptp_path_.sendGoal(move_ptp_path_goal);

  if (blocking_)
  {
    finished_before_timeout = ac_move_ptp_path_.waitForResult(ros::Duration(UR_PATH_EXECUTION_TIMEOUT));
    if (finished_before_timeout)
    {
      actionlib::SimpleClientGoalState state = ac_move_ptp_path_.getState();
      ROS_INFO("Action finished: %s", state.toString().c_str());
    }
    else
      ROS_INFO("Action did not finish before the time out.");
  }

  return finished_before_timeout;
}

bool URAIProxy::moveServoQ(const rw::math::Q& target, float time, float lookahead_time, float gain)
{
  bool finished_before_timeout = true;
  verifyValueIsWithin(lookahead_time, SERVO_LOOKAHEAD_TIME_MIN, SERVO_LOOKAHEAD_TIME_MAX);
  verifyValueIsWithin(gain, SERVO_GAIN_MIN, SERVO_GAIN_MAX);

  caros_universalrobot::MoveServoQGoal move_servo_q_goal;

  move_servo_q_goal.joint_pose = caros::toRos(target);
  move_servo_q_goal.time = caros::toRos(time);
  move_servo_q_goal.lookahead_time = caros::toRos(lookahead_time);
  move_servo_q_goal.gain = caros::toRos(gain);

  ac_move_servo_q_.sendGoal(move_servo_q_goal);

  if (blocking_)
  {
    finished_before_timeout = ac_move_servo_q_.waitForResult(ros::Duration(UR_EXECUTION_TIMEOUT));
    if (finished_before_timeout)
    {
      actionlib::SimpleClientGoalState state = ac_move_servo_q_.getState();
      ROS_INFO("Action finished: %s", state.toString().c_str());
    }
    else
      ROS_INFO("Action did not finish before the time out.");
  }

  return finished_before_timeout;
}

bool URAIProxy::moveServoStop()
{
  bool finished_before_timeout = true;

  caros_universalrobot::MoveServoStopGoal move_servo_stop_goal;

  ac_move_servo_stop_.sendGoal(move_servo_stop_goal);

  if (blocking_)
  {
    finished_before_timeout = ac_move_servo_stop_.waitForResult(ros::Duration(UR_EXECUTION_TIMEOUT));
    if (finished_before_timeout)
    {
      actionlib::SimpleClientGoalState state = ac_move_servo_stop_.getState();
      ROS_INFO("Action finished: %s", state.toString().c_str());
    }
    else
      ROS_INFO("Action did not finish before the time out.");
  }

  return finished_before_timeout;
}

bool URAIProxy::moveVelQ(const rw::math::Q& target, double acceleration, double time)
{
  bool finished_before_timeout = true;

  caros_universalrobot::MoveVelQGoal move_vel_q_goal;
  move_vel_q_goal.joint_velocity = caros::toRos(target);
  move_vel_q_goal.acceleration = caros::toRos(acceleration);
  move_vel_q_goal.time = caros::toRos(time);

  ac_move_vel_q_.sendGoal(move_vel_q_goal);

  if (blocking_)
  {
    finished_before_timeout = ac_move_vel_q_.waitForResult(ros::Duration(UR_EXECUTION_TIMEOUT));
    if (finished_before_timeout)
    {
      actionlib::SimpleClientGoalState state = ac_move_vel_q_.getState();
      ROS_INFO("Action finished: %s", state.toString().c_str());
    }
    else
      ROS_INFO("Action did not finish before the time out.");
  }

  return finished_before_timeout;
}

bool URAIProxy::moveVelT(const rw::math::VelocityScrew6D<>& target, double acceleration, double time)
{
  bool finished_before_timeout = true;

  caros_universalrobot::MoveVelTGoal move_vel_t_goal;
  move_vel_t_goal.tcp_velocity = caros::toRos(target);
  move_vel_t_goal.acceleration = caros::toRos(acceleration);
  move_vel_t_goal.time = caros::toRos(time);

  ac_move_vel_t_.sendGoal(move_vel_t_goal);

  if (blocking_)
  {
    finished_before_timeout = ac_move_vel_t_.waitForResult(ros::Duration(UR_EXECUTION_TIMEOUT));
    if (finished_before_timeout)
    {
      actionlib::SimpleClientGoalState state = ac_move_vel_t_.getState();
      ROS_INFO("Action finished: %s", state.toString().c_str());
    }
    else
      ROS_INFO("Action did not finish before the time out.");
  }

  return finished_before_timeout;
}

bool URAIProxy::stop()
{
  return false;
}

bool URAIProxy::sendCustomScriptFunction(const std::string& function_name, const std::string& script)
{
  bool finished_before_timeout = true;

  caros_universalrobot::SendCustomScriptFunctionGoal send_custom_script_function_goal;
  send_custom_script_function_goal.function_name = function_name;
  send_custom_script_function_goal.script = script;

  ac_send_custom_script_function_.sendGoal(send_custom_script_function_goal);

  if (blocking_)
  {
    finished_before_timeout = ac_send_custom_script_function_.waitForResult(ros::Duration(UR_EXECUTION_TIMEOUT));
    if (finished_before_timeout)
    {
      actionlib::SimpleClientGoalState state = ac_send_custom_script_function_.getState();
      ROS_INFO("Action finished: %s", state.toString().c_str());
    }
    else
      ROS_INFO("Action did not finish before the time out.");
  }

  return finished_before_timeout;
}

bool URAIProxy::sendCustomScriptFile(const std::string& file_path)
{
  bool finished_before_timeout = true;

  caros_universalrobot::SendCustomScriptFileGoal send_custom_script_file_goal;
  send_custom_script_file_goal.file_path = file_path;

  ac_send_custom_script_file_.sendGoal(send_custom_script_file_goal);

  if (blocking_)
  {
    finished_before_timeout = ac_send_custom_script_file_.waitForResult(ros::Duration(UR_EXECUTION_TIMEOUT));
    if (finished_before_timeout)
    {
      actionlib::SimpleClientGoalState state = ac_send_custom_script_file_.getState();
      ROS_INFO("Action finished: %s", state.toString().c_str());
    }
    else
      ROS_INFO("Action did not finish before the time out.");
  }

  return finished_before_timeout;
}

bool URAIProxy::zeroFtSensor()
{
  bool finished_before_timeout = true;

  caros_universalrobot::ZeroFTsensorGoal zero_ft_sensor_goal;

  ac_zero_ft_sensor_.sendGoal(zero_ft_sensor_goal);

  if (blocking_)
  {
    finished_before_timeout = ac_zero_ft_sensor_.waitForResult(ros::Duration(UR_EXECUTION_TIMEOUT));
    if (finished_before_timeout)
    {
      actionlib::SimpleClientGoalState state = ac_zero_ft_sensor_.getState();
      ROS_INFO("Action finished: %s", state.toString().c_str());
    }
    else
      ROS_INFO("Action did not finish before the time out.");
  }

  return finished_before_timeout;
}

rw::math::Q URAIProxy::getQ()
{
  return caros::toRw(robot_state_.q);
}

rw::math::Q URAIProxy::getQd()
{
  return caros::toRw(robot_state_.dq);
}

rw::math::Wrench6D<> URAIProxy::getTCPForce()
{
  return caros::toRw(robot_state_.tcp_force);
}

rw::math::Transform3D<> URAIProxy::getTCPPose()
{
  return caros::toRw(robot_state_.tcp_pose);
}

bool URAIProxy::isEmergencyStopped()
{
  return robot_state_.e_stopped;
}

bool URAIProxy::isSecurityStopped()
{
  return robot_state_.s_stopped;
}

ros::Time URAIProxy::getTimeStamp()
{
  return robot_state_.header.stamp;
}

void URAIProxy::handleRobotState(const caros_control_msgs::RobotState& state)
{
  robot_state_ = state;
}
